/*
 *
 * File Name          :  Sound.c
 * Description        :  Sound Functions
 */

#include <stdint.h>
#include <math.h>
#ifdef _STM32x_
#include "stm32f10x.h"
#include "circle_api.h"
#endif
#include "Sound.h"
#include "WavFile.h"
#include "Hrtf.h"
#include "HrtfDb.h"
#include "ClockCounter.h"

//#define SINWAVE_STEREO
//#define SOUND_WAV_ENABLE

#ifdef FIXED_POINT
#  define MAX_SIGNAL 32767
#  define MIN_SIGNAL -MAX_SIGNAL
#  define SOUND_FFT_SCALE 128
#else
#  define MAX_SIGNAL 1.0
#  define MIN_SIGNAL -MAX_SIGNAL
#  define SOUND_FFT_SCALE 0.002
#endif

int8_t _Sound_fEOF; //FlagEndOfFile
AUDIO_PlaybackBuffer_Status _Sound_BufferStatus;
SOUND_BUFFER_TYPE _Sound_AudioBuffer[SOUND_BUFFER_LENGTH];
SOUND_BUFFER_TYPE _Sound_StereoWavBuffer[SOUND_BUFFER_LENGTH];
HRTF_StereoSignal _Sound_StereoSignal;

int16_t Sound_Init(void) {
  _Sound_BufferStatus = LOW_EMPTY | HIGH_EMPTY ;
  _Sound_fEOF = 0;
  HRTF_Init();
  return 1;
}

const NUMBER_TYPE _Sound_Sinwave [] =
{
#include "Sinwave.h"
};

SOUND_BUFFER_TYPE *Sound_Get_AudioBuffer(void) { 
  return _Sound_AudioBuffer;
}

int32_t Sound_Get_AudioBuffer_Size(void) { 
  return SOUND_BUFFER_LENGTH;
}

int32_t Sound_Read(int16_t *dest, const int16_t *src, int32_t destLength, int32_t srcLength) {
    int32_t i, j, blockLength, currentPos;
    blockLength = destLength/srcLength;
    currentPos = 0;
    for(i=0; i<blockLength; i++) {
        for(j=0; j<srcLength; j++) {
            dest[currentPos] = src[j];
            currentPos++;
        } 
    }
    return currentPos;
}

int32_t Sound_ReadWavFile(FILEINFO *fileInfo, uint8_t *soundBuffer, uint32_t length) {
    uint32_t i, j, halfLength;
    uint8_t monoWavBuffer[2*DSP_FFT_SAMPLE_LENGTH];
    uint8_t *buffer;

    FS_ReadFile(fileInfo, (uint8_t *)monoWavBuffer, &i, length);
    buffer = (uint8_t *)monoWavBuffer;
    halfLength = i>>1;
    for(j=0; j<halfLength; j++) {
        soundBuffer[4*j] = buffer[2*j];
        soundBuffer[4*j+1] = buffer[2*j+1];
        soundBuffer[4*j+2] = 0;
        soundBuffer[4*j+3] = 0;
    }
    if(i==0) {
        FS_Seek(fileInfo, HEADER_SIZE);
        FS_ReadFile(fileInfo, (uint8_t *)monoWavBuffer, &i, length);        
        buffer = (uint8_t *)monoWavBuffer;
        halfLength = i>>1;
        for(j=0; j<halfLength; j++) {
            soundBuffer[4*j] = buffer[2*j];
            soundBuffer[4*j+1] = buffer[2*j+1];
            soundBuffer[4*j+2] = 0;
            soundBuffer[4*j+3] = 0;
        }
    }
    else if(i<length) {
        for(j=i; j<length; j++) {
            soundBuffer[j] = 0;
        }
        FS_Seek(fileInfo, HEADER_SIZE);
    }
    return length;
}

int32_t Sound_Copy(uint8_t *dst, uint8_t *src, int32_t length) {
    int32_t i;

    for(i=0; i<length; i++) {
        dst[i] = src[i];
    }

    return i;
}

int32_t Sound_Copy_16(int16_t *dst, const int16_t *src, int32_t length) {
    int32_t i;

    for(i=0; i<length; i++) {
        dst[i] = src[i];
    }

    return i;
}

float Sound_GetAzimuth() {
  static float azimuth = 0.0;
  float deltaAzimuth = 2.0;
  enum JOYSTICK_state jState;

  jState = JOYSTICK_GetState();
  if(jState==JOYSTICK_RIGHT)
    azimuth+=deltaAzimuth;
  else 
    if(jState==JOYSTICK_LEFT)
      azimuth-=deltaAzimuth;
  if(azimuth>HRTF_AZIMUTH_MAX)
    azimuth = HRTF_AZIMUTH_MAX;
  else if(azimuth<HRTF_AZIMUTH_MIN)
    azimuth = HRTF_AZIMUTH_MIN;
  
  return azimuth;
}

int32_t Sound_Position(SOUND_BUFFER_TYPE *dest, const int16_t *src, 
  int32_t destLength, int32_t srcLength, float elevation, float azimuth) { 
  int32_t i, currentPos;
  int32_t sig;
  char str1[16];
  
  STOPWATCH_START
        
  HRTF_SoundPosition(&_Sound_StereoSignal, (FPComplex *) src, srcLength, 0, azimuth);
 
  UTIL_int2str( &str1[0], (int)round(azimuth), 5, 1 );
  DRAW_DisplayString( 0, 90, str1, 6);  
  
  currentPos = 0;
  for(i=0; i<srcLength; i++) {
    sig = _Sound_StereoSignal.left[i][0]*SOUND_FFT_SCALE;
    if(sig>MAX_SIGNAL)
      sig = MAX_SIGNAL;
    else 
      if(sig<MIN_SIGNAL)
        sig = MIN_SIGNAL;
    dest[currentPos] = (SOUND_BUFFER_TYPE)sig;
    currentPos++;
    
    sig = _Sound_StereoSignal.right[i][0]*SOUND_FFT_SCALE;
    if(sig>MAX_SIGNAL)
      sig = MAX_SIGNAL;
    else 
      if(sig<MIN_SIGNAL)
        sig = MIN_SIGNAL;
    dest[currentPos] = (SOUND_BUFFER_TYPE)sig;
    currentPos++;
  }
  
  //Sound_Copy_16(dest, _Sound_Sinwave, 2048);
  STOPWATCH_STOP
  CycleCounter_Print(0, 130, 10);

  return currentPos;
}

enum MENU_code Sound_FillBuffer(FILEINFO *fileInfo) {
    int32_t i;
    int32_t half_audio_buffer = SOUND_BUFFER_LENGTH/2;
    _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(0);
    float azimuth;
    static float oldAzimuth = 180.0;
    static int cnt0 = 0, soundCounter = 0;
    static uint8_t str1[16];

    if ( _Sound_fEOF ) {
        if ( ( _Sound_BufferStatus & LOW_EMPTY ) && ( _Sound_BufferStatus & HIGH_EMPTY ) ) {
            DRAW_Clear();
            AUDIO_Playback_Stop();
            return MENU_Quit();
        }
        else 
            return MENU_CONTINUE;
    }

    TIM_Cmd( TIM2, DISABLE );
  
    azimuth = Sound_GetAzimuth();
    if(oldAzimuth != azimuth)
        soundCounter = 2;
    oldAzimuth = azimuth;
    soundCounter = 2;
    if( (_Sound_BufferStatus & LOW_EMPTY ) && soundCounter) {
    #ifdef SOUND_WAV_ENABLE
        //FS_ReadFile(fileInfo, (uint8_t *)_Sound_AudioBuffer, &i, half_audio_buffer);
        i = Sound_ReadWavFile(fileInfo, (uint8_t *)_Sound_AudioBuffer, 2*DSP_FFT_SAMPLE_LENGTH);
        //i = Sound_Read(_Sound_AudioBuffer, _Sound_StereoWavBuffer, half_audio_buffer, sizeof(_Sound_Sinwave)/sizeof(int16_t));
        //Sound_Position(_Sound_AudioBuffer, _Sound_StereoWavBuffer, half_audio_buffer, DSP_FFT_SAMPLE_LENGTH, 0, azimuth);
        _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(LOW_EMPTY);
    #else
        //FS_ReadFile(fileInfo, (uint8_t *)_Sound_AudioBuffer, &i, half_audio_buffer);
        //i = Sound_Read(_Sound_AudioBuffer, _Sound_Sinwave, half_audio_buffer, sizeof(_Sound_Sinwave)/sizeof(int16_t));
        i = Sound_ReadWavFile(fileInfo, (uint8_t *)_Sound_StereoWavBuffer, 2048);
        Sound_Position(_Sound_AudioBuffer, _Sound_StereoWavBuffer, half_audio_buffer, DSP_FFT_SAMPLE_LENGTH, 0, azimuth);
        //Sound_Copy((uint8_t *)_Sound_AudioBuffer, (uint8_t *)_Sound_Sinwave, 2*half_audio_buffer);
        _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(LOW_EMPTY);
        
        if ( i < half_audio_buffer ) {
            _Sound_fEOF  = 1;
        /*        
            for((i/=sizeof(int16_t));i<(half_audio_buffer/2);i++)
                _Sound_AudioBuffer[i] = 0;
        */
        }
    #endif
        soundCounter--;
    }

    if( (_Sound_BufferStatus & HIGH_EMPTY )  && soundCounter){
    #ifdef SOUND_WAV_ENABLE
        //FS_ReadFile(fileInfo, (uint8_t *)_Sound_AudioBuffer+half_audio_buffer, &i, half_audio_buffer);
        i = Sound_ReadWavFile(fileInfo, (uint8_t *)_Sound_AudioBuffer+2*half_audio_buffer, 2*DSP_FFT_SAMPLE_LENGTH);
        //i = Sound_Read(_Sound_AudioBuffer+half_audio_buffer, _Sound_StereoWavBuffer, half_audio_buffer, sizeof(_Sound_Sinwave)/sizeof(int16_t));
        //Sound_Position(_Sound_AudioBuffer+half_audio_buffer, _Sound_StereoWavBuffer+half_audio_buffer, half_audio_buffer, DSP_FFT_SAMPLE_LENGTH, 0, azimuth);
        _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(HIGH_EMPTY);
    #else
        //FS_ReadFile(fileInfo, (uint8_t *)_Sound_AudioBuffer+half_audio_buffer, &i, half_audio_buffer);
        //i = Sound_Read(_Sound_AudioBuffer+half_audio_buffer, _Sound_Sinwave, half_audio_buffer, sizeof(_Sound_Sinwave)/sizeof(int16_t));
        i = Sound_ReadWavFile(fileInfo, (uint8_t *)_Sound_StereoWavBuffer, 2048);
        Sound_Position(_Sound_AudioBuffer+half_audio_buffer, _Sound_StereoWavBuffer, half_audio_buffer, DSP_FFT_SAMPLE_LENGTH, 0, azimuth);
        //Sound_Copy((uint8_t *)_Sound_AudioBuffer+2*half_audio_buffer, (uint8_t *)_Sound_Sinwave, 2*half_audio_buffer);
        _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(HIGH_EMPTY);
        
        if ( i< half_audio_buffer ) {
            _Sound_fEOF  = 1;         
            /*
            for((i/=sizeof(int16_t));i<(half_audio_buffer/2);i++)
                _Sound_AudioBuffer[i+half_audio_buffer/2] = 0;
            */
        }
    #endif
        soundCounter--;
    }

    cnt0++;
    if(cnt0%10==0) {
        UTIL_int2str( &str1[0], cnt0, 5, 1 );
        DRAW_DisplayString( 0, 60, str1, 6);
    }
    TIM_Cmd( TIM2, ENABLE );

    return MENU_CONTINUE;
}